#ifndef __DWA_H__
#define __DWA_H__

#include "controller/controller_base.h"

// DWA配置参数
struct DWAConfig{
    double linear_sampling_resolution = 0.1;     //线速度采样分辨率
    double angular_sampling_resolution = 0.2;    //角速度采样分辨率
    double predicted_time = 2.0;     //预测时间
    double time_sampling_resolution = 0.2;    //时间采样分辨率
};


// 轨迹
struct Trajectory{
    vector<geometry_msgs::PoseStamped> waypoints;     //bsee_frame坐标系下的预测轨迹
    geometry_msgs::Twist cmd_vel;   //速度数据
    double traj_score;             //轨迹得分
    double heading_score;          //航向角得分
    double speed_score;            //速度得分
    double goal_score;             //终点距离得分
    double obstacle_score;         //障碍物得分
};


// DWA控制器
class DWAController: public ControllerBase {
    public:
        DWAController();
        ~DWAController();

        mc::status computeTwist(mc::TrajectoryPoint2D goal, geometry_msgs::Twist curr_vel,
            geometry_msgs::Twist& cmd_vel);

        // // 构建速度空间
        // void buildVelocitySpace();
        // // 计算预测轨迹
        // void predictTraj();
        // // 发布点云数据
        // void publishPointCloud2(vector<Trajectory> predicted_trajs);
        // // 发布最佳轨迹
        // void publishBestTraj(Trajectory best_traj);

        // // 计算航向角度评分  轨迹末端点角度和末端点与目标点连线的角度差
        // double headingScore(geometry_msgs::PoseStamped traj_end_point, 
        //     geometry_msgs::PoseStamped goal);
        // // 速度评分
        // double velocityScore(geometry_msgs::Twist cmd_vel);

        // // 计算障碍物评分  计算轨迹末端点距离最近障碍物距离的评价函数 碰撞检测
        // bool obstaclesScore(geometry_msgs::PoseStamped traj_end_point, 
        //     geometry_msgs::Twist vel_2d, 
        //     vector<geometry_msgs::PoseStamped>& obstacles, 
        //     double inflation_radius, double& min_distance);
        // // 碰撞检测  简单的距离计算，后期优化
        // bool obstaclesCollision(vector<geometry_msgs::PoseStamped> waypoints, 
        //     vector<geometry_msgs::PoseStamped>& obstacles, double inflation_radius);   

        // bool obstaclesScoreByCostMap(vector<geometry_msgs::PoseStamped>& waypoints); 

        // // 查找最佳轨迹
        // bool findBestTrajectory(Trajectory& best_traj, geometry_msgs::PoseStamped goal);


        // // 计算速度
        // mc::status computeTwist(geometry_msgs::PoseStamped goal, geometry_msgs::Twist curr_vel, geometry_msgs::Twist& cmd_vel);

    private:
        ros::NodeHandle nh_;
        DWAConfig dwaConfig_;
        ros::Publisher dwaPredictedTrajPub_;
        ros::Publisher dwaBestTrajPub_;

        vector<geometry_msgs::Twist> velSpace_;         //速度空间
        vector<Trajectory> predictedTraj_;   //速度空间对应的轨迹缓存
};

#endif
